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[57] ABSTRACT 

A redundant robot control scheme is provided for 
avoiding obstacles in a workspace during motion of an 
end effector along a preselected trajectory by stopping 
motion of the critical point on the robot closest to the 
obstacle when the distance therebetween is reduced to a 
predetermined sphere of influence surrounding the ob- 
stacle. Algorithms are provided for conveniently deter- 
mining the critical point and critical distance. 

1 Claim, 2 Drawing Sheets 
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OBSTACLE AVOIDANCE FOR REDUNDANT 
ROBOTS USING CONFIGURATION CONTROL 

ORIGIN OF THE INVENTION 

The invention described herein was made in the per- 
formance of work under a NASA contract, and is sub- 
ject to the provisions of Public Law 96-517 (35 USC 
202) in which the Contractor has elected not to retain 
title. 

1. Technical Field 

The present invention relates to the robotic systems 
and, in particular, to the operation of robotic systems to 
avoid obstacles while positioning end effectors. 

2. Background of the Invention 

Conventional obstacle avoidance during end effector 

positioning in robotic systems is included in the high 
level or task programming for the system. Dynamic or 
real time obstacle avoidance has been difficult to 
achieve in part because of the vast amounts of data that 
would have to be communicated between the high level 
processing unit and the robot servo systems. 

Robotic systems which are capable of motion in more 
degrees of freedom than required for the programmed 
task are known as redundant robots. A simple example 
of a redundant robot is a robotic arm capable of posi- 
tioning an end effector in an 2 dimensional plane pro- 
grammed to position the end effector at points along a 
line in that plane. An approach to the control of redun- 
dant robots is disclosed in a copending patent applica- 
tion filed Dec. 28, 1989, U.S. Ser. No. 07/459,029 in the 
name of one of the inventors hereof and assigned to a 
common assignee, is known as configuration control. 
Configuration control provides a convenient technique 
for beneficially utilizing the redundancy in such sys- 
tems. 

What is needed is a dynamic, real time technique 
which utilizes the inherent qualities of redundant robots 
for obstacle avoidance. 

BRIEF STATEMENT OF THE INVENTION 

The preceding and other shortcomings of the prior 
art are addressed and overcome by the present inven- 
tion that provides, in a first aspect, a method of operat- 
ing a redundant robot system to position an end effector 
in a workspace by changing joint angles between links 
connected to the end effector, determining the location 
of an obstacle in the workspace, determining a sphere of 
influence having a fixed radius surrounding the obsta- 
cle, determining a critical point, on a link, closest to the 
sphere of influence, determining a critical distance be- 
tween the critical point and the obstacle, and operating 
the robot system to stop motion of the critical point 
toward the obstacle when the critical distance equals 55 
the radius. 

In another aspect, the present invention provides a 
redundant robot system including an end effector, inter- 
connected link means for positioning the end effector in 
a workspace, means for locating the position of an ob- 60 
stacle in the workspace, means for determining a critical 
point on the link means closest to the obstacle; means 
for continuously determining a critical distance be- 
tween the critical point and the obstacle, means for 
operating the robot system to position the end effector 65 
along a preselected trajectory in the workspace and 
stopping motion of the critical point when the critical 
distance is reduced to a predetermined minimum. 


2 

These and other features and advantages of this in- 
vention will become further apparent from the detailed 
description that follows which is accompanied by a set 
of drawing figures. In the figures and description, nu~ 

5 merals indicate the various features of the invention, 
like numerals referring to like features throughout both 
the drawings and the description. 

BRIEF DESCRIPTION OF THE DRAWING(S) 

10 FIG. 1 is a block diagram of a redundant robotic 
system according to the present invention using config- 
uration control for obstacle avoidance. 

FIG. 2 is a generalized representation of a robot link 
and obstacle pair. 

DETAILED DESCRIPTION OF THE 
INVENTION 

Referring now to FIG. 1, robotic system 10 includes 
host computer 12 connected by bidirectional digital bus 
20 14 to robotic interface 16 which processes the host 
computer instructions to provide the appropriate com- 
mands to robotic arm system 20 via cable 18. Robotic 
arm system 20 includes base 22 and end effector 24 
interconnected by links 26, 28 and 30. Robotic arm 
25 system 20 is used to position end effector 24 in accor- 
dance with preprogrammed task instructions from host 
computer 12 to permit the performance of a task. 

Robotic arm system 20 controls the position of end 
effector 24 by adjusting joint angles 0j, 02 and 03 be- 
30 tween base 22 and link 26, between links 26 and 28 and 
between links 28 and 30, respectively. Adjustment of 
these angles permits motion of end effector 24 with 
more degrees of freedom than required for positioning 
of end effector 24 in the plane of the figure. Robotic arm 
35 system 20 is therefore considered to be a redundant 
robot. 

The redundancy of robotic system 10 is used to good 
advantage in accordance with the present invention to 
permit low level, real time obstacle avoidance. Obstacle 
40 32 is potentially in the path of motion of link 26 if, for 
example, host computer 12 issued instructions to robotic 
interface 16 to move end effector 24 from its position as 
shown in the figure to position 38. Proper articulation of 
links 26, 28 and 30 will permit the positioning of end 
45 effector 24 at position 38, but care must be taken to 
insure that all of the links, particularly link 26, avoids 
obstacle 32. 

In order to insure safety, a spherical zone around 
obstacle 32 having a radius “r” is designated surround- 
50 ing obstacle 32 as its sphere of influence or SOI 34. 
Links 26, 28 and 30 are prevented from entering SOI 34 
to guarantee that they do not contact obstacle 32. 

The motion of links 26, 28 and 30 and therefore all 
points thereon may be determined as a function of the 
articulation of robotic arm system 20, that is, as a func- 
tion of joint angles 0i, 02 and 03- In accordance with the 
present invention, robotic interface 16 determines the 
point or points, shown as in the figure as critical point 
40, on link 26, 28 and/or on link 30, whichever point is 
closest to obstacle 32. The distance between obstacle 32 
and critical point 40 is designated as critical distance 36. 

Critical point 40 is continuously determined as ro- 
botic arm system 20 moves links 26, 28 and 30. Critical 
distance 36 continuously represents the shortest be- 
tween obstacle 32 and any point on links 26, 28 and 30. 

When critical distance 36 is reduced by motion of 
robotic arm system 20 to equal the value r, the radius of 
SOI 34, a collision is imminent. In accordance with the 
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present invention, articulation of robotic arm system 20 
is then constrained to prevent further reduction of criti- 
cal distance 36. Of course, further articulation of ro- 
botic arm system 20 may result in a change of critical 
point 40 along link 26 or to another link. Obstacle 
avoidance is achieved as long as the proper inequality 
constraints are maintained so that critical distance 36, 
from whichever is the critical point closest to SOI 34, is 
equal to or greater than radius r. 

Within host computer 12 are end-effector trajectory 
portion 44 which includes the programming which 
identifies the intended path of end effector 24 required 
in order to perform the task at hand. End-effector tra- 
jectory portion 44 provides the trajectory information 
to end effector controller 46 which generates the com- 
mands necessary for robotic interface 16 to properly 
position end effector 24. 

In addition the above described portions of a conven- 
tional host computer for robot arm control, host com- 
puter 12 according to the present invention includes 
obstacle avoidance criterion 48 which identifies and 
stores the information necessary to identify and avoid 
obstacles, such as obstacle 32, including the appropriate 
radius for SOI 34 and the locations of the obstacles. 

The criterion provided by obstacle avoidance crite- 
rion 48 are applied to obstacle avoidance controller 50, 
together with end effector trajectory information from 
end effector controller 46. The robotic interface control 
information from end effector controller 46 and obsta- 
cle avoidance controller 50 is combined and applied to 
robotic interface 16 on bidirectional bus 14 to provide 
end effector positioning information together with 
servo control instructions permitting obstacle avoid- 
ance. 

The operation of robotic system 10 may be best de- 
scribed in conjunction with the following brief over- 
view of the configuration control scheme for redundant 
robots, taken from the article entitled “Obstacle Avoid- 
ance for Redundant Robots Using Configuration Con- 
trol” written by the present inventors and published the 
Journal of Robotic Systems, Vol. 6, No. 6, pp 721-745, 
December, 1989. That article provides additional exam- 
ples of the operation of the present invention. 

The robot manipulator under consideration, such as 45 
robotic arm system 20, consists of a linkage of rigid 
bodies with n revolute/prismatic joints. Let T eR" be 
the vector of joint torques/forces and 0cRn be the vec- 
tor of rotation/translations. The dynamic model of ro- 
botic arm system 20 can be derived from Lagrangian 
mechanics as 


25 


30 


35 


40 
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dL 

00 


( 1 ) 


( 0 , 0 ) 


= H(6)B + V(6)e + Gi6) 


( 2 ) 


where L(*)eR is the Lagrangian of the robotic arm sys- 
tem 20, and HeR rtXn and V, GeR” are complicated 
nonlinear functions of 6 , A, and end effector 24. Let 
XeR m , with m<n, define the position and orientation of 
end effector 24 in the task space. The relationship be- 
tween the end-effector coordinate X and the joint coor- 
dinate 0 can be written as 


where fR"— >K m represents forward kinematic transla- 
tion. Differentiation of equation (3) above with respect 
to time yields 


x=-M0)0 


(4) 


where J e =df/d6eR nXm is the Jacobian of robotic arm 
system 20. 

Configuration control provides for the global control 
of redundant robots such as robotic system 10 and pre- 
scribes the selection of a generalized coordinate vector 
YeR" that is more task-relevant than the joint coordi- 
nate vector 0. This configuration vector Y may be con- 
trolled globally across the entire workspace by ensuring 
that Y(t) tracks a desired trajectory Y^t) using host 
computer 12. 

The vector Y is defined as: 


■-M 


(5) 


where ZeR r , and r—n — m is the degree-of-redundancy 
of the robotic arm system 20. 

The vector Z is chosen as 


Z = g(0) 


( 6 ) 


where g:R"~ *R r is a kinematic vector function con- 
structed to reflect the performance of some additional 
useful task, such as obstacle avoidance. Observe that 
specifying g and the desired evolution of Y defines the 
obstacle avoidance task to be performed in addition to 
the basic task of positioning end effector 24. 

The dynamic model of robotic arm system 20 can be 
derived in terms of the configuration vector Y since this 
vector is a valid generalized coordinate vector for ro- 
botic arm system 20. Proceeding in accordance with 
equations (1) and (2) shown above yields 




dL 

6Y 


(Y.Y) 


= H/Y)Y+ V/Y.Y) + Gy{Y) 


(7) 


( 8 ) 


where FeK n is the generalized force vector correspond- 
ing to the generalized coordinate vector Y, and 
H^eR" xw and \ y , G^eR" are complicated nonlinear 
functions of Y, Y and end effector 24. 

The centralized control algorithm that ensures that 
the manipulator control configuration vector Y(t) as 
shown in equation (8) tracks the desired trajectory Y^(t) 


55 


F=d(t)+Kfl)E+K&)E+B(f)Y d +C(t) d +A(t)Y d 


(9) 


60 


65 


where E=Yd—Y is the configuration tracking error, 
and deR w and K^, K v , C, B, AeR WXn are controller gains 
which are generated on line in real time according to 
the following simple adaptation laws: 


q = WpE + WvE 


( 10 ) 


d(t) = d( 0 ) + 


j 0 


qdt + h\q 


X = f(0) 


(3) 
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-continued 


Kp(i) = Kp( 0) -f *2 

f qE’di 4- hiqE 

J 0 

K^t) = K,(0) + * 3 

f qE'di + h-i,qE 


J 0 

r 

i 

C{t) = C(0) + *4 j 

qYd'dt 4- h^qYd 

0 

BU) = B( 0) + *5 J 

' . 

qYd'dt + hf>qYd' 

0 

A(t) = A( 0) + k b j 

t 

qYd'dt 4- h^qYdi 

0 


In equation (10), the k/and h/ terms are positive and 
nonnegative scalar constant adaptation gains, respec- 
tively, which are chosen to provide the desired adapta- 
tion rates for the controller terms. The constant, usually 
diagonal, weighting matrices W^, Wv€R” Xw are selected 
to reflect the relative significance of the individual ele- 
ments of the tracking error vectors E and E . Under the 
control laws in accordance with equations (9) and (10), 
the desired trajectory for end effector 24, x<Xt), is 
tracked and the extra degrees of freedom are appropri- 
ately used to control the evolution of the configuration 
of robotic arm system 20 through the tracking of the 
desired kinematic trajectory Z^t). 

Note that the control force F is computed entirely 
based on the observed performance of robotic arm sys- 
tem 20 rather than on the dynamic model of robotic arm 
system 20 as shown in equation (8). The online adapta- 
tion of the controller using equation (10) eliminates the 
need for the complicated mathematical model of the 
dynamics of robotic arm system 20. This relieves the 
designer from the derivation, on line computation and 
knowledge of parameters of the complicated robot dy- 
namic model. The simplicity of this control scheme 
allows the designer to implement very fast control loops 
and thereby improve system performance. 

The control force F computed in equations (9) and 
(10) cannot be physically applied to robotic arm system 
20 and must be mapped to an equivalent joint torque 
vector T, in accordance with known techniques. 

In the foregoing analysis, the additional task which 
may be performed as a result of the redundancy may be 
formulated as the kinematic inequality constraints 

g(0)^o (11) 

These inequality constraints may readily be incorpo- 
rated into the configuration control scheme. In order to 
satisfy the inequality constraints shown in equation (1 1), 
the reference trajectory may be defined as Z^t)=0. 
The tracking errors due to these constraints are given 
by 

E c — 0, E c =0 when g(0)5O 

£ c = -g, E c = -g when g(8)<0 (12) 

Therefore, in the additional task controller, the feed- 
forward term is omitted and the feedback control action 
may be computed as 

F c ^d(t)+K^t)E c +K^A (13) 


where d, K^. K v are the adaptive controller terms given 
in equation (10) and updated based on the tracking 
errors in accordance with equation (13). It is important 
5 to note that both equality and inequality constraints can 
exist simultaneously in a given additional task provided 
the total number of active constraints is not greater than 
the degree of redundancy r. Using this formulation, the 
additional task to be performed by redundant robotic 
10 arm system 20 can be decomposed into a number of 
subtasks with different sets of r kinematic constraints, 
such as different obstacles to be avoided. In the execu- 
tion of each subtask, the appropriate kinematic con- 
straint is satisfied in addition to the desired motion of 
15 end effector 24. 

It is important to note that when the number of kine- 
matic constraints c is less than r, the configuration con- 
trol scheme will automatically use the r— c extra de- 
grees of redundancy to minimize the robotic kinetic 
20 energy integrated over the entire trajectory. This is a 
very desirable feature of global optimality in many 
applications. 

The problem of obstacle avoidance is to ensure that 
links 26, 28 and 30 do not collide with obstacles in the 
25 workplace, such as obstacle 32, while robotic arm sys- 
tem 20 moves end effector 24 along a desired, pre- 
planned trajectory to perform a task. In accordance 
with the present invention, the obstacle avoidance crite- 
ria may be formulated as a set of kinematic inequality 
30 constraints in the tasks space. The configuration control 
scheme is used to ensure that these inequality con- 
straints are satisfied while the desired trajectory for end 
■ effector 24 is tracked. 

As noted above, obstacle 32 is enclosed in SOI 34, a 
35 convex volume with sufficient volume surrounding 
obstacle 32 to provide protection so that transient errors 
will not cause a collision. The basic strategy is to inhibit 
motion of critical point 40 toward obstacle 32 when 
critical point 40 enters SOI 34. 

40 The approach to obstacle avoidance provided herein 
may be described in greater detail with respect to FIG. 
2 which provides a generalized representation of a 
robot link and obstacle pair similar to link 26 and obsta- 
cle 32 as shown in FIG. 1. Referring therefore now to 
45 FIG. 2, link i of length 1/is shown together with obstacle 
j with its sphere of influence SOI. 

Define (X c )ije R J to be the position of critical point 42 
on link i relative to obstacle j as measured on the base 
frame of the robot system, not shown, where critical 
50 point 42 is that point on link i currently at a minimum 
distance from obstacle j. Here i = l, 2, . . n and there 
are k obstacles so that j = l, 2, . . . ,k. Let (X 0 )y€ R 3 and 
(r o)j denote the position of the center and the radius of 
obstacle j, respectively. Then using 
55 

(14) 

wmij = 1 1 (Xc)ij - (*o)j\ i = m c )ij - mj)\(x c )ij - 

the criterion for obstacle avoidance many be expressed 
60 as a set of inequality constraints: 

[dmij - (ro)j ~ *#) § 0 ; ( 15 ) 

i = 1 , 2 , = 1 , 2 ,...,* 

65 

For a moving obstacle, (Xo)j and therefore g jj are 
functions of time, and hence the inequality constraints 
of equations (15) must be satisfied for all time t. The 
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constraint ij given in equation (15) is defined to be ac- 
tive if g ; )<0, and no more than r constraints are simulta- 
neously active at any time. If the number of active con- 
straints c is less than r, the configuration control scheme 
automatically uses the r— c additional degrees of redun- 
dancy to minimize robot kinetic energy integrated over 
the entire trajectory. 

In the event that more than r constrains are simulta- 
neously active, that is c>r, it is appropriate to abort the 
task, because in general there is no safe trajectory solu- 
tion to the tracking problem in this case. This does not 
prevent operation in a workspace containing more than 
r obstacles, provided that only r are handled at any one 
time. In view of this approach, the currently active 
constraints may be stacked into the constraint vector 

g( 0 .t)eo (i6) 

with giR^—^R^ where c = r and the explicit time depen- 
dency of g is shown to accommodate moving obstacles. 
The inequality constraint shown in equation (16) is the 
same as the constraint shown in equation (11), and 
therefore this constraint relationship and the desired 
trajectory for end effector 24 can be tracked simulta- 
neously using the configuration control law provided in 
equations (9) through (14). In order to implement this 
scheme, however, an efficient method for locating the 
active critical points must be developed, and expres- 
sions for E c , E c , and J c must be derived as shown below. 

In constructing an algorithm to locate active critical 
points, it is important to note that the locations of the 
critical points vary during the robot task and must be 
continuously updated. Thus the algorithm must be com- 
putationally efficient. The location of all the critical 
points, that is, the points on each link closest to each 
obstacle, must be determined. Then the determination is 
made if any of these critical points are active, that is, 
within any obstacle SOIs. 

Referring now again to FIG. 2, X/eR 3 is defined to be 
the location of joint i relative to a reference, such as the 
base frame supporting the link, a//eR+ to be the distance 
along link i from joint i to critical point 
and ei=(Xi+]~Xj)/Ii. These definitions may be used to 
derive the following recursive algorithm for computing 
the location of all active critical points: 


a ij — tfPij — y [%i 4- 1 — Xj\'\{X 0 )j — Xj\ 


if ay si 0 then ay = 0 

(17) 

if ay = lj then ay = lj 


( X c )ij — Xj -f o.ySi 

(18) 

(d c )y = m c )y - (X 0 )j)X(X c )ij) - (X o )0 

(19) 

If the inequality 


(d C )y<(r 0 y 

(20) 


is true, then (X c )y is an active critical point, otherwise it 
is not active. 

Having located the active critical points using the 
algorithms presented in equations (17) through (20), the 
constraint vector provided in equation (16) may readily 
be constructed from the definition provided in equation 
(15). The fact that all of the constraints included in the 
constraint vector provided in equation (16) are active 


8 

makes the calculations of E c eK c and Ee c R c straightfor- 
ward, that is, the case E c ,/==0 need not be considered. 

The expressions for E c , E c , and J f may then be de- 
rived as follows: 


10 


15 


E c = [£ f j. / = 1. 2, . . . , c; (21) 

with E c j = (r 0 )i - (d c ) 

E c = [£ c ./], 1 = 1, 2, . . . , c, with 


E e j 


( *8i * *8i ^ 

- g = - « +— J 

1 T HXe)i • * "I 

- [« - oar l~je- 6 - J- 


where the subscript i refers to element i of g in equa- 
tion (16); e.g. (X c )i and (X 0 )/ are the critical point — 
20 obstacle pair corresponding to element i of g, and (d c )/ 
is the distance between them. Note that g/ is simply th'e 
projection, of the critical point — obstacle approach 
velocity (X c )/— (X 0 ), onto the unit vector pointing from 
25 (X 0 )i to (Xc)/. 

The Jacobian constraint matrix J c eR cXri may be com- 
puted row by row' through direct differentiation of the 
elements of g in equation (16): 


30 (23) 

«■* = ~bT = llch [(jyi - <*** [ ^TT- } 

The c rows (J c )/ are then stacked to form J c . The 
35 matrix (Jjc) c /= a(X c )//a6eR 3x/7 is recognized as the Jaco- 
bian of the critical point (X c ), in the base frame. The 
matrix (Jjc) c / can be computed very efficiently for any 
(X c )/ once the Jacobians of all the robot joints are 
knowm. The rows of J c can therefore be computed effi- 
40 ciently as 


(Jc), = toy, - ay,] • (j x ) ci (24) 

45 / = 1, 2, . . . , c. 

While this invention has been described with refer- 
ence to its presently preferred embodiment, its scope is 
not limited thereto. Rather, such scope is only limited 
50 insofar as defined by the following set of claims and 
includes all equivalents thereof. 

What is claimed is: 

1. A method of controlling a redundant robot, com- 
55 prising the steps of: 

defining end effector coordinates to position an end 
effector in a workspace by changing joint angles 
between links connected to the end effector; 

determining spheres of influence having fixed radii 
60 surrounding obstacles at known positions in the 
workspace; 

defining kinematic tasks for collision avoidance cor- 
responding to additional task motion capability 
available as a result of the redundancy of the robot, 
65 said kinematic tasks including 

(a) continuously determining a first critical point 
on a link closest to a closest one of said spheres of 
influence; 
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(b) defining a first critical distance related to the 
distance between the first critical point and the 
obstacle within said closest sphere of influence; 

(c) forming inequality constraints in which the first 5 
critical distance is greater than the radius of said 
closest sphere of influence; and 

combining said end effector coordinates with said 
kinematic tasks to form task related configuration io 
variables for control of said redundant robot; 

dynamically modifying said joint angles while operat- 
ing the robot system to position the end effector in 
the work space while maintaining said inequality 15 
constraints by stopping motion of the first critical 
point toward said first obstacle when the first criti- 
cal distance equals the radius of said closest sphere 
of influence; 

modifying said kinematic tasks to then include 


10 

(d) continuously determining a second critical 
point related to a closest point on a link closest to 
a next closest sphere of influence; 

(e) defining a second critical distance related to the 
distance between the second critical point and a 
second obstacle, said second obstacle being in 
said next closest sphere of influence; 

(0 forming additional inequality constraints in 
which the second critical distance is greater than 
the radius of said next closest sphere of influence; 
and 

dynamically modifying said joint angles while operat- 
ing the robot system to position the end effector in 
the work space while continuing to stop motion of 
said first critical point toward said first obstacle 
and maintaining said additional inequality con- 
straints by stopping motion of the second critical 
point toward said second obstacle when the second 
critical distance equals the radius of said next clos- 

20 est sphere of influence. 

***** 
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